/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "CTopoOccuMCL.h"

CTopoOccuMCL::CTopoOccuMCL(){
		O=NULL;
		dist = NULL;
		prob_var = 0;
		prob_max = 0;
		measurement_variance = 1.0;
		vmeas = measurement_variance*measurement_variance;
}
CTopoOccuMCL::~CTopoOccuMCL(){
	
	if(dist) free(dist);
	if(O) delete(O);
}

/**
 * Initializes the filter - uses normal random method for calculating the initial distribution
 * Loads the map from file, reserves memory for particles, sets the initial distribution
 * @p *mapfile The mapfile name
 * @p mapscale the scale factor needed to get the map coordinates into meters
 * @p num_of_particles The number of particles in the filter
 * @p init_pose the initial pose
 * @p initVariance the variance for the initial quess
 * @p num_of_meas_in_scan number of measurements used from the scan
 * @p resolution The resolution of the occupancy grid (in meters)
 * @p lineval The value the line map is set to in grid
 */
void CTopoOccuMCL::initialize(const char *mapfile,
								float mapscale, 
								int num_of_particles, 
								mcl::pose initPose, 
								mcl::pose initVariance,
								float resolution,
								grid_t lineval){
										
		initializeVariables(mapfile,mapscale,resolution,lineval);
				
		filt.initializeNormalRandom(initPose, initVariance,num_of_particles);													
}
/**
 * Initializes the filter - uses normal random method for calculating the initial distribution
 * Loads the map from line file, reserves memory for particles, sets the initial distribution
 * @p *mapfile The mapfile name
 * @p num_of_particles The number of particles in the filter
 * @p init_pose the initial pose
 * @p initVariance the variance for the initial quess
 * @p num_of_meas_in_scan number of measurements used from the scan
*/
void CTopoOccuMCL::initialize(const char *mapfile,
								int num_of_particles, 
								mcl::pose initPose, 
								mcl::pose initVariance,
								grid_t occu_val)
{
	///< initializing with small value, will be resized when linemap is read
	O = new TOccupancyGrid(1,1,0.1,Grid::TCoord2D(0,0));
	 
	if(!O->load(mapfile)){
		fprintf(stderr,"CTopoOccuMCL::initializeVariables::Unable to load the map\n");
		exit(1);
	}
	o_val = occu_val;
	int Nx,Ny;
	O->getDim(Nx, Ny);
	dist = (float *) malloc(Nx*Ny*sizeof(float));
	fprintf(stderr,"CTopoOccuMCL::initializeVariables(): Computing distance transform\n");
	O->distanceTransform(dist, o_val);	 
	
	float *dataPtr=O->getDataPtr();;
	///Now lets make us a Likelihood function
	for(int i=0;i<Nx*Ny;i++){
		if(dist[i]>6) dist[i]=0;
		if(dist[i]>0.3) dist[i]=0.3; ///Everything further than 0.3m away from obstacles get the same value
		
		dataPtr[i] = dist[i]; 			///< Copy to occupancy grid
	}
	///Thats it :) Now we have a precomputed 2D likelihood function
	
	filt.initializeNormalRandom(initPose, initVariance,num_of_particles);
	fprintf(stderr,"CTopoOccuMCL::Variables initialized\n");
}

/**
 * Initializes the filter - uses uniform distribution method for calculating the initial distribution
 * Loads the map from file, reserves memory for particles, sets the initial distribution
 * The initial distribution is calculated using bounding box of the map. The @dP sets the 
 * resolution of the unform grid.  WARNING the initialization might not be 
 * real-time.
 * @p *mapfile The mapfile name
 * @p mapscale the scale factor needed to get the map coordinates into meters
 * @p dP the resolution for the uniform grid
 * @p num_of_meas_in_scan number of measurements used from the scan
 */
void CTopoOccuMCL::initializeGlobal(const char *mapfile,
																		float mapscale,
															float resolution,  
							 								mcl::pose dP, 
															mcl::pose start,
							 								mcl::pose end)
{
		initializeVariables(mapfile,mapscale,resolution,(grid_t) 1.0);
		filt.initializeUniform(start, end,dP);
}							

/**
 * Initialises the variables
*/
void CTopoOccuMCL::initializeVariables(const char *mapfile,
															float mapscale,  
															float resolution,
															grid_t lineval)
{
	///< initializing with small value, will be resized when linemap is read
	O = new TOccupancyGrid(1,1,resolution,Grid::TCoord2D(0,0)); 
	if(!O->loadLineMap(mapfile, mapscale,lineval)){
				fprintf(stderr,"CTopoOccuMCL::initializeVariables::Unable to load the map\n");
				exit(1);
		}
		o_val = lineval;
		int Nx,Ny;
		O->getDim(Nx, Ny);
		dist = (float *) malloc(Nx*Ny*sizeof(float));
		fprintf(stderr,"CTopoOccuMCL::initializeVariables(): Computing distance transform\n");
		O->distanceTransform(dist, lineval);	 
		float *dataPtr=O->getDataPtr();;
	///Now lets make us a Likelihood function
		for(int i=0;i<Nx*Ny;i++){
			if(dist[i]>8) dist[i]=0;
			//if(dist[i]>3) dist[i]=(3/dist[i])*0.3;
			if(dist[i]>8) dist[i]=(3/dist[i])*0.3;
			if(dist[i]>0.2) dist[i]=0.3; ///Everything further than 0.3m away from obstacles get the same value
			dataPtr[i] =  dist[i]; 			///< Copy to occupancy grid
		}
	///Thats it :) Now we have a precomputed 2D likelihood function
		
		fprintf(stderr,"CTopoOccuMCL::Variables initialized\n");
}
/**
 * Same as init but without mallocs
 */
void CTopoOccuMCL::reinitializeNR(mcl::pose initPose,mcl::pose initVariance){
		filt.initializeNormalRandom(initPose, initVariance,filt.NumOfParticles);	
}

/**
 * Run the MCL step
 * @p dP the relative movement between steps
 * @p variance prediction error 
 * @p measurement the scan taken from new position
 */
void CTopoOccuMCL::runMCL(mcl::pose dP,mcl::pose variance, mcl::scan measurement){
		mcl::scan mcl_meas;
		mcl::pose var;
		static float sinceSIR=0; 
		//float sensor_offset_tmp;
		
		mcl_meas.allocate(1);
		mcl_meas.N=1;
		
		///Take the measurements that will be used in estimation
		/// The number is reduced for computational & efficiency reasons i.e. MCL must not be "too accurate"
	
		calculateLikelihood(dP); ///<Calculate likelihoods
		filt.normalize(); 
		filt.predict(dP,variance); ///<Predict
	
		//OLD:: calculateLikelihood(); ///<Calculate likelihoods
		
		
		var = filt.getDistributionVariances(); 
		//float rad = sqrt(var.x*var.x + var.y*var.y + var.a*var.a);
		
		///Calculate the variance of the probabilities
		float varP=0;
		float iN = 1.0 / (float) filt.NumOfParticles;
		float maxP = -1;
		for(int i = 0; i<filt.NumOfParticles;i++){
				varP += (filt.Particles[i].p - iN)*(filt.Particles[i].p - iN);
				if(filt.Particles[i].p > maxP) maxP = filt.Particles[i].p;
		}
		prob_max = maxP;
		prob_var = varP;
		///Using now stetson harrison method
		//fprintf(stderr,"%f \n",varP);
		if((varP > .0001 || sinceSIR > 100)&&sinceSIR>2){
				//fprintf(stderr,"Performing SIR varP=%.3f\n",varP);
				filt.SIRUpdate();
				sinceSIR = 0;
				prob_var = 0;
				prob_max = 1/filt.NumOfParticles;
		}
		sinceSIR++;
		
		free(mcl_meas.r);
		free(mcl_meas.a);
}


/**
 * Run the MCL step without ever doing SIR 
 * @p dP the relative movement between steps
 * @p variance prediction error 
 * @p measurement the scan taken from new position
 */
void CTopoOccuMCL::runMCLnoSIR(mcl::pose dP,mcl::pose variance, mcl::scan measurement){
		mcl::scan mcl_meas;
		mcl::pose var;
		static float sinceSIR=0; 
		//float sensor_offset_tmp;
		
		mcl_meas.allocate(1);
		mcl_meas.N=1;
		
		///Take the measurements that will be used in estimation
		/// The number is reduced for computational & efficiency reasons i.e. MCL must not be "too accurate"
	
		filt.predict(dP,variance); ///<Predict
	
		calculateLikelihood(); ///<Calculate likelihoods
		
		filt.normalize(); 
		var = filt.getDistributionVariances(); 
		//float rad = sqrt(var.x*var.x + var.y*var.y + var.a*var.a);
		
		free(mcl_meas.r);
		free(mcl_meas.a);
}


/**
 * Run the MCL step in initialization mode
 * @p dP the relative movement between steps
 * @p variance prediction error 
 * @p measurement the scan taken from new position
 */
void CTopoOccuMCL::runMCLInit(mcl::pose dP,mcl::pose variance, mcl::scan measurement){
		mcl::scan mcl_meas;
		mcl::pose var;
		static int sinceSIR=2;
	
		
		
		mcl_meas.allocate(1);
		mcl_meas.N=1;
		
		
		filt.predict(dP,variance); ///<Predict
		calculateLikelihood(); ///<Calculate likelihoods
		filt.normalize(); 
		var = filt.getDistributionVariances(); 

		filt.SIRUpdate(); ///< allways update!
		sinceSIR++;
		
		free(mcl_meas.r);
		free(mcl_meas.a);
}


/**
 * Calculates the likelihood of particle distribution
 * uses the CParticleFilter filt and TLineMap map
 * @param meas The current measurement
 **/
void CTopoOccuMCL::calculateLikelihood()
{
		int i;		
		filt.Lik = 0;
		filt.outLiers = 0;
		///Cycle through each particle
		//fprintf(stderr,"Numofmeas=%d, num particles=%d\n",num_of_meas,filt.NumOfParticles);
		grid_t v;
		
		for(i=0;i<filt.NumOfParticles;i++){	
				O->getValue(Grid::TCoord2D(filt.Particles[i].x, filt.Particles[i].y),v);
				filt.Lik+=v;
				filt.Particles[i].lik=v;
		}
}

/**
 * Calculates the likelihood of particle distribution
 * Uses the distribution before the prediction
 * @param dP The dead reckoning measurement
 **/
void CTopoOccuMCL::calculateLikelihood(mcl::pose dP)
{
		int i;		
		filt.Lik = 0;
		filt.outLiers = 0;
		///Cycle through each particle
		//fprintf(stderr,"Numofmeas=%d, num particles=%d\n",num_of_meas,filt.NumOfParticles);
		grid_t v;

		float dl = sqrt(dP.x*dP.x+dP.y*dP.y); ///< movement
		float da = atan2(dP.y,dP.x);
		float reso = O->getResolution();
		float ddl,dx,dy;
		Grid::TCoord2D c;
		float lik;
		for(i=0;i<filt.NumOfParticles;i++){
				ddl=0;
				lik=1;
				while(ddl<dl){
						c.x = ddl*cos(filt.Particles[i].a + da) + filt.Particles[i].x;
						c.y = ddl*sin(filt.Particles[i].a + da) + filt.Particles[i].y;
						O->getValue(c,v);
						if(lik>v) lik=v;
						ddl+=reso;
				}
				filt.Particles[i].lik=lik;
				filt.Lik+=filt.Particles[i].lik;
		}
}



void CTopoOccuMCL::getScan(mcl::scan &meas, mcl::pose &p){
	Grid::TScan out;
	out.alloc(meas.N);
	
	O->getScan(p.x,
						 p.y, 
						 p.a+sensor_offset, 
						 meas.N, 
						 M_PI/meas.N, 
						 o_val,
						 dist, 
						 out);
			
	float a=sensor_offset;
	float da=(M_PI)/meas.N;
	
	for(int i = 0;i<meas.N;i++){
		meas.r[i] = out.r[i];
		meas.a[i] = a;
		a+=da;
	}
	
  out.freeData();
}
void CTopoOccuMCL::updateDt(){
	O->distanceTransform(dist, o_val);	
}